/*
 * CoarseAlignmentSM.c
 *
 *  Created on: Nov 30, 2012
 *      Author: gmkado
 */

/*****************************************************
 * Include Statements
 ****************************************************/
#include "CoarseAlignmentSM.h"
#include "inc/lm3s2965.h"
#include "URF.h"
#include "Servo.h"

//TODO: CALIBRATE THESE
#define RANGE_JUMP_THRESHOLD 10
#define RANGE_DIFF_TOL 3
#define URF_ANGLE_OFFSET 5
#define URF_ID 1
#define STEP_ANGLE 1
/*****************************************************
 * Module Level Variables
 ****************************************************/
// Module level variable contains the current state
static CoarseAlignmentSMState_t currentState;
static range_t currRange, driverRange, passengerRange;
static angle_t currAngle, driverAngle, passengerAngle;

/*****************************************************
 * Initialization Function
 ****************************************************/
void InitCoarseAlignmentSM(void)
{
	// All initialization connected to this SM goes here
	currentState = FindDriverSide;
	initURF();
	initServo();
	currRange = getURFRange(URF_ID);
	currAngle = getServoAngle();
}

/*****************************************************
 * Run Function
 ****************************************************/
void RunCoarseAlignmentSM(void)
{
	angle_t prevAngle;
	range_t prevRange;

	prevAngle = currAngle;
	prevRange = currRange;

	switch(currentState)
	{
		case FindDriverSide:
			puts("CoarseAlignment:FindDriverSide");
			/* move CW until a jump is seen or servo angle passes 0 degrees */
			moveServoCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange(URF_ID);

			if(currAngle<MIN_ANGLE)
			{
				// never got a range jump, report vehicle out of range

				// TODO: Throw OUT_OF_RANGE event

			}
			else if(abs(currRange-prevRange) > RANGE_JUMP_THRESHOLD)
			{
				driverRange = prevRange;
				driverAngle = prevAngle;
				currentState = ConfirmDriverSide;
			}

		break;
		case ConfirmDriverSide:
			/* move CCW until driverRange is seen (within a tolerance)
			 * and take the average of the current angle/range with the prev angle/range
			 */
			puts("CoarseAlignment:FindDriverSide");
			moveServoCCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange(URF_ID);

			if(currAngle > SERVO_HOME)	{
				// we went passed the middle, something went wrong
				// TODO: start over???
			}
			if(abs(currRange - driverRange) < RANGE_DIFF_TOL)
			{
				/* take the average of the two values seen
				 * Compensate for URF angle offset
				 */
				driverRange = (driverRange + currRange)/2;
				driverAngle = (driverAngle + currAngle)/2 + URF_ANGLE_OFFSET;
				currentState = FindPassengerSide;
				moveServo(SERVO_HOME);
				currAngle = SERVO_HOME;
				currRange = getURFRange(URF_ID);	// TODO: Need to wait until servo gets home before getting range
			}
		break;
		case FindPassengerSide:
			puts("CoarseAlignment:FindDriverSide");
			/* move CW until a jump is seen or servo angle passes 0 degrees */
			moveServoCCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange(URF_ID);

			if(currAngle>MAX_ANGLE)
			{
				// never got a range jump, report vehicle out of range

				// TODO: Throw OUT_OF_RANGE event

			}
			else if(abs(currRange-prevRange) > RANGE_JUMP_THRESHOLD)
			{
				passengerRange = prevRange;
				passengerAngle = prevAngle;
				currentState = ConfirmPassengerSide;
			}

		break;
		case ConfirmPassengerSide:
			puts("CoarseAlignment:FindDriverSide");
			/* move CCW until driverRange is seen (within a tolerance)
			 * and take the average of the current angle/range with the prev angle/range
			 */
			moveServoCW(STEP_ANGLE);

			currAngle = getServoAngle();
			currRange = getURFRange();

			if(currAngle < SERVO_HOME)	{
				// we went passed the middle, something went wrong
				// TODO: start over???
			}
			if(abs(currRange - passengerRange) < RANGE_DIFF_TOL)
			{
				/* take the average of the two values seen
				 * Compensate for URF angle offset
				 */
				passengerRange = (passengerRange + currRange)/2;
				passengerAngle = (passengerAngle + currAngle)/2 + URF_ANGLE_OFFSET;
				currentState = FindDriverSide;
				moveServo(SERVO_HOME);
				currAngle = SERVO_HOME;
				currRange = getURFRange();	// TODO: Need to wait until servo gets home before getting range
				// TODO: Throw event COARSE_ALIGNMENT_COMPLETE
			}
		break;
}

